#include "BoundingVolume.h"
#include "Plane.h"

namespace Nezha
{

	void AABB::merge(const Vector3& v)
	{
		mMin.x = NZMin(v.x, mMin.x);
		mMin.y = NZMin(v.y, mMin.y);
		mMin.z = NZMin(v.z, mMin.z);
		mMax.x = NZMax(v.x, mMax.x);
		mMax.y = NZMax(v.y, mMax.y);
		mMax.z = NZMax(v.z, mMax.z);
	}

	void AABB::mergeSphere(const Vector3& c, float r)
	{
		Vector3 v(c.x - r, c.y - r, c.z - r);
		mMin.x = NZMin(v.x, mMin.x);
		mMin.y = NZMin(v.y, mMin.y);
		mMin.z = NZMin(v.z, mMin.z);

		v.set(c.x + r, c.y + r, c.z + r);
		mMax.x = NZMax(v.x, mMax.x);
		mMax.y = NZMax(v.y, mMax.y);
		mMax.z = NZMax(v.z, mMax.z);
	}

	void AABB::mergeAABB(const AABB& aabb)
	{
		mMin.x = NZMin(aabb.mMin.x, mMin.x);
		mMin.y = NZMin(aabb.mMin.y, mMin.y);
		mMin.z = NZMin(aabb.mMin.z, mMin.z);
		mMax.x = NZMax(aabb.mMax.x, mMax.x);
		mMax.y = NZMax(aabb.mMax.y, mMax.y);
		mMax.z = NZMax(aabb.mMax.z, mMax.z);
	}

	void AABB::fit2(const AABB& aabb)
	{
		mMin.x = NZMax(aabb.mMin.x, mMin.x);
		mMin.y = NZMax(aabb.mMin.y, mMin.y);
		mMin.z = NZMax(aabb.mMin.z, mMin.z);
		mMax.x = NZMin(aabb.mMax.x, mMax.x);
		mMax.y = NZMin(aabb.mMax.y, mMax.y);
		mMax.z = NZMin(aabb.mMax.z, mMax.z);
	}

	void AABB::translate(const Vector3& v)
	{
		float scalar = _guaranteeValid(1.0f, 0);
		if(scalar > 0)
		{
			mMin += v;
			mMax += v;
		}
	}

	// sphere
	bool AABB::intersectSphere(const Vector3& c, float r) const
	{
		if(r <= 0 || empty() || notValid())
			return false;

		// Arvo's method
		float quatradius = r * r;
		Vector3 quat(0, 0, 0);

		if(c.x < mMin.x) { quat.x = c.x - mMin.x; } 
		else if(c.x > mMax.x) {	quat.x = c.x - mMax.x;}
		if(c.y < mMin.y) { quat.y = c.y - mMin.y; }	
		else if(c.y > mMax.y) {	quat.y = c.y - mMax.y; }
		if(c.z < mMin.z) { quat.z = c.z - mMin.z;} 
		else if(c.z > mMax.z) {	quat.z = c.z - mMax.z; }

		return quat.sqrLength() < quatradius;
	}

	bool AABB::intersectAABB(const AABB& b) const
	{
		if(empty() || _notValid() || b.empty() || b._notValid())
			return false;

		if ((mMin.x > b.mMax.x)||(b.mMin.x > mMax.x)) return false;
		if ((mMin.y > b.mMax.y)||(b.mMin.y > mMax.y)) return false;
		if ((mMin.z > b.mMax.z)||(b.mMin.z > mMax.z)) return false;

		return true;
	}

	bool AABB::intersectPlane(const Vector3& normal, float d) const
	{
		if(empty() || notValid())
			return false;

		Plane tmpPlane(normal, d);

		return Plane::SIDE_BOTH == tmpPlane.whichSide(*this);
	}

	void AABB::rebuildAffine(const Matrix4x4& affineM)
	{
		if(!_notValid())
		{
			Vector3 c = affineM.affine(getCenter());
			Vector3 oldsz = getSize() * 0.5f;

			// rotate only, but turn to the 'max' axis.
			Vector3 sz(fabs(oldsz.x * affineM[0][1]) + fabs(oldsz.y * affineM[0][2]) + fabs(oldsz.z * affineM[0][3]),
				fabs(oldsz.x * affineM[0][1]) + fabs(oldsz.y * affineM[0][2]) + fabs(oldsz.z * affineM[0][3]),
				fabs(oldsz.x * affineM[0][1]) + fabs(oldsz.y * affineM[0][2]) + fabs(oldsz.z * affineM[0][3]));

			mMin = c - sz;
			mMax = c + sz;
		}
	}

	void AABB::rebuild(const Matrix4x4& mat)
	{
		Vector3 oldMin, oldMax, currentCorner;

		oldMin = mMin;
		oldMax = mMax;

		reset();

		// min min min
		currentCorner = oldMin;
		merge(mat.project(currentCorner));

		// min,min,max
		currentCorner.z = oldMax.z;
		merge(mat.project(currentCorner));

		// min max max
		currentCorner.y = oldMax.y;
		merge(mat.project(currentCorner));

		// min max min
		currentCorner.z = oldMin.z;
		merge(mat.project(currentCorner));

		// max max min
		currentCorner.x = oldMax.x;
		merge(mat.project(currentCorner));

		// max max max
		currentCorner.z = oldMax.z;
		merge(mat.project(currentCorner));

		// max min max
		currentCorner.y = oldMin.y;
		merge(mat.project(currentCorner));

		// max min min
		currentCorner.z = oldMin.z;
		merge(mat.project(currentCorner));
	}

	AABB AABB::transform(const Matrix3x3& rot, const Vector3& trans) const
	{
		AABB aabb;

		if(!_notValid())
		{
			Vector3 sz = rot * ((mMax - mMin) * 0.5f);
			Vector3 c = (rot * ((mMax + mMin) * 0.5f)) + trans;

			aabb.mMin = c - sz;
			aabb.mMax = c + sz;
		}

		return aabb;
	}

	AABB AABB::transform(const Matrix4x4& affineM) const
	{
		AABB aabb;

		if(!_notValid())
		{
			Vector4 sz = affineM * Vector4((mMax - mMin) * 0.5f, 1.0f);
			Vector4 c = affineM * Vector4((mMax + mMin) * 0.5f, 1.0f);
			sz.x -= affineM[0][3];
			sz.y -= affineM[1][3];
			sz.z -= affineM[2][3];

			aabb.mMin.x = c.x - sz.x;
			aabb.mMin.y = c.y - sz.y;
			aabb.mMin.z = c.z - sz.z;
			aabb.mMax.x = c.x + sz.x;
			aabb.mMax.y = c.y + sz.y;
			aabb.mMax.z = c.z + sz.z;
		}

		return aabb;
	}

	AABB AABB::transform(const Quaternion& quat) const
	{
		AABB aabb;

		if(!_notValid())
		{
			Matrix3x3 rot;
			quat.toRotation(rot);

			Vector3 sz = rot * ((mMax - mMin) * 0.5f);
			Vector3 c = rot * ((mMax + mMin) * 0.5f);

			aabb.mMin = c - sz;
			aabb.mMax = c + sz;
		}

		return aabb;
	}

	void AABB::fromVertexsData(u32 numVerts, u32 offsetInVertex, u32 stride, u8* data)
	{
		reset();

		data += offsetInVertex;

		for(u32 i = 0; i < numVerts; i ++)
		{
			const Vector3* v = (Vector3*)(data + i * stride);
			merge(*v);
		}
	}

}//end namepsace Nezha